#include "RigidPoly.h"
#include "RigidCircle.h"

namespace poly
{
	RigidPoly::RigidPoly()
	{
	}

	RigidPoly::RigidPoly( D3DDevice d3ddev, 
		RigidBody const & rb, Polygon const & poly, float4 const & color )
	{
		rigidBody = rb;
		m_poly = poly;
		m_view.initialize( d3ddev );
		m_view.setup( poly, color );
	}

	void RigidPoly::step(float dt)
	{
		rigidBody.step(dt);
	}

	void RigidPoly::render(float dt)
	{
		m_view.render( rigidBody.world() );
	}

	Polygon RigidPoly::polygon() const
	{
		return m_poly;
	}

	void RigidPoly::setup( D3DDevice d3ddev, Polygon const & poly, float4 const & color )
	{
		
		m_view.destroy();
		m_view.initialize( d3ddev );
		m_view.setup( poly, color );
		m_poly = poly;
	}

	void RigidPoly::collide( IRigidCollider * collider )
	{
		collider->collide( this );
	}

	void RigidPoly::collide( RigidPoly * rp )
	{
		poly::Polygon polyi = m_poly.transform( rigidBody.world() );
		poly::Polygon polyj = rp->m_poly.transform( rp->rigidBody.world() );

		Contact contact;
		if( intersect( contact, polyi, polyj ) )
		{
			rigidBody.resolveCollision( rp->rigidBody, contact );
		}
	}

	void RigidPoly::collide( RigidCircle * rc )
	{
		poly::Polygon polyi = m_poly.transform( rigidBody.world() );
		float3 circleCenter = rc->rigidBody.pos;
		float circleRadius = rc->radius;

		Contact contact;
		if( intersect( contact, polyi, circleCenter, circleRadius ) )
		{
			rigidBody.displace( -contact.seperation / 2 );
			rc->rigidBody.displace( +contact.seperation / 2 );

			rigidBody.resolveCollision( rc->rigidBody,
				contact );
		}

	}
}
